#pragma once
#include <memory>
#include "pfsm_state.h"
namespace TP {
class TP_Itf;
typedef struct TP_Config_Stable TP_Config_Stable;
}  // namespace TP

namespace axisgroup {
class PFsmCoord : public PFsmState
{
public:
    PFsmCoord(Robot* grp);
    int update_plan() override;
    int enter() override;
    void exit() override { return; }
    void abort() override;
    bool is_plan_over() override;
    bool is_stopping() override;
    int init() override;
    void pause() override;
    void resume() override;
    bool is_paused() const override;
    int update_to_axisgroup();
    int set_tp_config(TP::TP_Config_Stable& tp_config_);

public:
    std::shared_ptr<TP::TP_Itf> tp_intf_;

private:
    int update_tp_constrain();
    int checkTpError();

    int finish_warn_failed;
    int finish_warn_cross_safearea;
    int distance_to_go;
    int current_vel;
    int enables_queued;
    int dtg;
};
}  // namespace axisgroup